#region Using Statements
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Content.Pipeline;
using Microsoft.Xna.Framework.Content.Pipeline.Graphics;
using Microsoft.Xna.Framework.Content.Pipeline.Processors;
using System.Collections;
using System.Collections.Generic;
#endregion

namespace CollisionProcessor
{
    /// <summary>
    /// The main method in charge of processing the content.
    /// In this case processed from NodeContent to a CollisionObject
    /// A CollisionObject is a List CollisionConvexHulls
    ///   A CollisionConvexHull is a List of ConvexSides
    ///     A CollisionSide is a List of vertices, starting with the normal, followed by all points
    /// </summary>
    [ContentProcessor]
    public class CollisionProcessor : ContentProcessor<NodeContent, List<Dictionary<string, object>>>
    {
        List<Dictionary<string, object>> _collisionHull = new List<Dictionary<string, object>>();

        /// <summary>
        /// The main method, called to process from NodeContent to a nested list of all normals
        /// and vertices that make up the collision object
        /// </summary>
        /// <param name="input">Input provided by the content importer</param>
        /// <param name="context">Provides access to custom processor parameters</param>
        /// <returns>Collision object</returns>
        public override List<Dictionary<string, object>> Process(NodeContent input, ContentProcessorContext context)
        {
            FindCollisionObject(input);
            return _collisionHull;
        }

        /// <summary>
        /// Process a node object and makes calls to process childs nodes
        /// </summary>
        /// <param name="node">Node to process</param>
        void FindCollisionObject(NodeContent node)
        {
            // Is this node a mesh?
            MeshContent mesh = node as MeshContent;

            if (mesh != null)
            {
                // Look up the absolute transform of the mesh.
                Matrix absoluteTransform = mesh.AbsoluteTransform;

                // Loop over all the pieces of geometry in the mesh. Separete convex hulls are pieces of geometry
                foreach (GeometryContent geometry in mesh.Geometry)
                {
                    _collisionHull.Add(FindConvexHull(geometry, absoluteTransform));
                }
            }

            // Recursively scan over the children of this node.
            foreach (NodeContent child in node.Children)
            {
                FindCollisionObject(child);
            }
        }

        /// <summary>
        /// Processes one ConvexHull, by reading triangles from geometry and clustering them
        /// to form sides. All the sides together are the hull and are returned
        /// </summary>
        /// <param name="geometry">Geometry to process</param>
        /// <param name="absoluteTransform">World transform matrix</param>
        /// <returns>Convex Hull</returns>
        Dictionary<string, object> FindConvexHull(GeometryContent geometry, Matrix absoluteTransform)
        {
            Dictionary<string, object> convexHull = new Dictionary<string, object>();

            //Add all vertices to the hull
            List<Vector3> vertices = new List<Vector3>();
            foreach(Vector3 vertex in geometry.Vertices.Positions)
                vertices.Add(vertex);
            int[] conv;
            vertices = Flatten(vertices, 0.01f, out conv);
            convexHull.Add("vertices", vertices);
            //Add name to hull (does not work)
            convexHull.Add("name", geometry.Name);
            //Initialze list with side
            List<Dictionary<string, object>> hullSide = new List<Dictionary<string, object>>();

            Vector3 normal = new Vector3();
            Vector3[] triangle = new Vector3[3]; //Triangle + normal
            int[] indices = new int[3];

            int count = 0;
            // Loop over all the indices in this piece of geometry.
            // Every group of three indices represents one triangle.
            foreach (int oldIndex in geometry.Indices)
            {
                int index = conv[oldIndex];
                // Look up the position of this vertex.
                Vector3 vertex = vertices[index];

                // Transform from local into world space.
                vertex = Vector3.Transform(vertex, absoluteTransform);

                // Store this vertex.
                indices[count] = index;
                triangle[count++] = vertex;
                if (count == 3) //Added three vertices, calculate normal
                {
                    count = 0;
                    normal = Vector3.Cross(triangle[1] - triangle[0], triangle[1] - triangle[2]);
                    normal.Normalize();
                    //Find other convex with equal normal
                    bool merged = false;
                    foreach (Dictionary<string, object> side in hullSide)
                    {
                        //If found, then add new vertex to convex side
                        if(EqualNormal((Vector3)side["normal"], normal))
                        {
                            merged = true;
                            for (int i = 0; i < 3; i++) //Loop through all vertices in triangle
                            {
                                bool found = false;
                                foreach(int j in (List<int>)side["indices"])  //Loop through all vertices in side
                                {
                                    if (EqualVector3(triangle[i], vertices[j], 0.01f))
                                        found = true;
                                }
                                //This convexSide vector3 is not in side, therefore add
                                if (!found)
                                    //((List<int>)side["indices"]).Add(indices[i]);
                                    InsertIndex(((List<int>)side["indices"]), vertices, indices[i]);
                            }
                        }
                    }
                    if (!merged) //If not found, then add ConvexSide to ConvexShape
                    {
                        float d = -Vector3.Dot(normal, triangle[0]);
                        Dictionary<string, object> side = new Dictionary<string, object>();
                        List<int> lIndices = new List<int>(indices);
                        side.Add("indices", lIndices);
                        side.Add("normal", normal);
                        side.Add("d", d);
                        hullSide.Add(side);
                    }
                }
            }
            convexHull.Add("hullside", hullSide);
            return convexHull;
        }

        /// <summary>
        /// Returns bary centric coordinates of point p, in respect to edge e0 and e1
        /// </summary>
        /// <param name="e0">First edge (origin at 0,0,0)</param>
        /// <param name="e1">Second edge (origin at 0,0,0)</param>
        /// <param name="p">Point to convert</param>
        /// <returns>Barycentric coordinates</returns>
        Vector2 ToBaryCentricCoordinates(Vector3 e0, Vector3 e1, Vector3 p)
        {
            float dot00 = Vector3.Dot(e0, e0);
            float dot01 = Vector3.Dot(e0, e1);
            float dot02 = Vector3.Dot(e0, p);
            float dot11 = Vector3.Dot(e1, e1);
            float dot12 = Vector3.Dot(e1, p);

            float invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
            float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
            float v = (dot00 * dot12 - dot01 * dot02) * invDenom;
            return new Vector2(u, v);
        }

        /// <summary>
        /// Inserts an index in such a way that the border of the side is traversed in order
        /// </summary>
        /// <param name="indices">List of existing indices</param>
        /// <param name="vertices">Vertices to which the indices points</param>
        /// <param name="index">Index to add</param>
        void InsertIndex(List<int> indices, List<Vector3> vertices, int index)
        {
            Vector3 baseV = vertices[indices[0]];
            Vector2 bary;
            for (int i = 1; i < indices.Count-1; i++)
            {
                bary = ToBaryCentricCoordinates(vertices[indices[i]]-baseV,
                                                        vertices[indices[i+1]]-baseV,
                                                        vertices[index]-baseV);
                if (bary.X >= 0 && bary.Y >= 0)
                {
                    indices.Insert(i + 1, index);
                    return;
                }
            }
            baseV = vertices[indices[indices.Count - 1]];
            bary = ToBaryCentricCoordinates(vertices[indices[0]] - baseV,
                                                    vertices[indices[1]] - baseV,
                                                    vertices[index] - baseV);
            if (bary.X >= 0 && bary.Y >= 0)
            {
                indices.Insert(1, index);
                return;
            }
            indices.Add(index);
        }

        /// <summary>
        /// Removes double from vertices
        /// </summary>
        /// <param name="vertices"></param>
        /// <param name="small"></param>
        /// <returns>Conversion table</returns>
        List<Vector3> Flatten(List<Vector3> vertices, float small, out int[] conv)
        {
            List<Vector3> output = new List<Vector3>();
            conv = new int[vertices.Count];

            for (int current = 0; current < vertices.Count; current++)
            {
                conv[current] = current;
                Vector3 v = vertices[current];
                //Find v in identified uniques
                bool found = false;
                for (int unique = 0; unique < output.Count; unique++)
                {
                    if (EqualVector3(output[unique], v, small))
                    {
                        found = true;
                        conv[current] = unique;
                        break;
                    }
                }
                //if not found and new unique vertex
                if (!found)
                {
                    conv[current] = output.Count;
                    output.Add(v);
                }
            }
            return output;
        }

        /// <summary>
        /// Determines if two normals are similar enough to consider equal
        /// They are considered when angle between them is small enough
        /// </summary>
        /// <param name="v1">First vector</param>
        /// <param name="v2">Second vector</param>
        /// <returns>True when normals are similar enough, false otherwise</returns>
        bool EqualNormal(Vector3 v1, Vector3 v2)
        {
            return (Vector3.Dot(v1, v2) > 0.995);
        }

        /// <summary>
        /// Determines if two vector are similar enough to consider equal
        /// Two vectors are considered equal when they are close enough to each other
        /// </summary>
        /// <param name="v1">First vector</param>
        /// <param name="v2">Second vector</param>
        /// <returns>True when vectors are close to each other, false otherwise</returns>
        bool EqualVector3(Vector3 v1, Vector3 v2, float small)
        {
            return ((Vector3)(v1 - v2)).Length() < small;
        }

        /// <summary>
        /// Sorts the vertices on a convex side. The first Vector3 is ignored (this is the normal)
        /// The second and third vertices are used to provide reference to sort the remaining vertices
        /// </summary>
        /// <param name="list"></param>
        /*void Sort(List<int> indices, ref List<Vector3> vertices)
        {
            AngleOfVerticesComparer tc = new AngleOfVerticesComparer(ref vertices, indices);
            indices.Sort(2, indices.Count - 2, tc);
        }*/
    }
}
